/*
 * main.cpp
 *
 *  Created on: Nov 9, 2012
 *      Author: bouchier
 */
#define DEFAULT_TEST 2

#include <stdio.h>
#include <stdlib.h>
#include "qegpioint.h"
#include "Metro.h"
#include "usonicRanger.h"

CQEGpioInt &gpio = CQEGpioInt::GetRef();
struct usonicRanger *lRanger;		// left ranger
struct usonicRanger *cRanger;		// center ranger
struct usonicRanger *rRanger;		// right ranger

void usage()
{
	printf("Usage: usonicRanger testNum\n");
	printf("testNum can be:\n");
	printf("1: print pulse-widths\n");
}

void test1()
{
	// ACHTUNG - CHECK THE DIO NUMBERS
	// initialize the steering, speed & estop R/C structs & start interrupt monitoring
	lRanger = initUsonicRanger(gpio, 12, 11);		// initialize left usonic ranger on DIO 12 (Trig - IN) & 11 (Sense - Out)
	triggerUsonicRanger(lRanger);

	Metro metro = Metro(50);
	while (1){
		if (metro.check()) {
			printf("LRange: %d\n", lRanger->distance);
			triggerUsonicRanger(lRanger);				// trigger it to pulse again
		}
	}
}

static int currentUsonicRanger = 0;
void test2()
{
	// ACHTUNG - CHECK THE DIO NUMBERS
	// initialize the steering, speed & estop R/C structs & start interrupt monitoring
	lRanger = initUsonicRanger(gpio, 12, 11);		// initialize left usonic ranger on DIO 12 & 11
	cRanger = initUsonicRanger(gpio, 14, 13);		// initialize center usonic ranger on DIO 14 & 13
	rRanger = initUsonicRanger(gpio, 16, 15);		// initialize right usonic ranger on DIO 16 & 15
	triggerUsonicRanger(lRanger);				// start lRanger running

	Metro metro = Metro(50);
	while (1){
		if (metro.check()) {
			switch (currentUsonicRanger) {
			case 0:
				triggerUsonicRanger(cRanger); currentUsonicRanger++; break;
			case 1:
				triggerUsonicRanger(rRanger); currentUsonicRanger++; break;
			case 2:
				triggerUsonicRanger(lRanger); currentUsonicRanger=0;
				printf("Ranges: L: %d, C: %d, R: %d\n", lRanger->distance, cRanger->distance, rRanger->distance);
				break;
			}
		}
	}
}

int main(int argc, char **argv)
{
	int test = DEFAULT_TEST;	// which test to run
	int argval;		// value to pass into test

	if (argc == 2)
		test = atoi(argv[1]);
	if (argc == 3)
		argval = atoi(argv[2]);
	printf("Running test %d\n", test);

	switch (test) {
	case 1:	test1(); break;
	case 2: test2(); break;
	//case 3: test3(); break;
	//case 4: test4(); break;
	default:
		printf("Invalid option\n");
		usage();
	}
}
